#include "GroupByColorAlgorithm.hpp"

const double GroupByColorAlgorithm::offset = 3.14/4; // turn if more than 45 deg from center

GroupByColorAlgorithm::GroupByColorAlgorithm() : SwarmAlgorithm() {
    id = "GROUPBYCOLOR";
}
    
bool GroupByColorAlgorithm::setup() {
    if (red->getIsOn()) {
        color = "RED";
    }
    else if (green->getIsOn()) {
        color = "GREEN";
    }
    else if (blue->getIsOn()) {
        color = "BLUE";
    }
    
    return true;
}

void GroupByColorAlgorithm::loopBody() {
    static int c = LED::colorStringToInt(color);
    static int intelCount = 0; // how long to continue last intelligence

    // assess the situation
    robots = robotFinder->getRobotPositions();
    unsigned char action = ACTION_STOP;

    // handle bumper issues
    if(bumperControl->isFrontTouching())
    {
        if (bumperControl->isBackTouching())
        {
            motorControl->stop();
            return;
        }

        int i = getBumperRobotIndex(robots);
        if (i > -1 && strcmp(color.c_str(), robots[i].label) == 0) { // bumping our robot
            motorControl->stop();
            return;
        }
        
        // bumping another robot
        action = ACTION_REVERSE;
    }
    else if (bumperControl->isBackTouching()) {
        int i = getBumperRobotIndex(robots, false);
        if (i > -1 && strcmp(color.c_str(), robots[i].label) == 0) {
            motorControl->stop();
            return;
        }

        action = ACTION_FORWARD;
    }
    else {
        unsigned char newAction = ClusterAlgorithm::clusterFurthestAction(c);
        if(newAction != ACTION_STOP)
        {
            action = newAction;
            intelCount = 3;
        }
        else if (intelCount > 0) {
            intelCount--;
        }
        else {
            action = getRandomMotorAction();
        }
    }

    performAction(action, 5, 5);
}
